#include "tim.h"
#include "n20.h"
#include "PID.h"
#include "arm_math.h"
#include "shooter.h"
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
#define N20_EN

N20 n20;

//void N20_Init()
//{
//	HAL_GPIO_WritePin(GPIOE, GPIO_PIN_14, GPIO_PIN_RESET);//将PB14置低电平
//	HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6, GPIO_PIN_RESET);//将PF1置低电平

//}


//void N20_Out()
//{
//	HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, GPIO_PIN_RESET);//将PB14置低电平
//	HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_SET);//将PF1置高电平
//}

//void N20_In()
//{
//	HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, GPIO_PIN_SET);//将PB14置高电平
//	HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_RESET);//将PF1置低电平
//}

//void N20_Stop()
//{
//	HAL_GPIO_WritePin(GPIOE, GPIO_PIN_9, GPIO_PIN_RESET);//将PB14置低电平
//	HAL_GPIO_WritePin(GPIOE, GPIO_PIN_11, GPIO_PIN_RESET);//将PF1置低电平

//}



void N20_TaskOut()
{
 n20.targetAngle+=500;
}


void N20_TaskIn()
{
 n20.targetAngle-=1000;
	

	
}

	
void N20_Init()
{

    HAL_TIM_Base_Start_IT(&htim5);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
    HAL_TIM_Encoder_Start(&htim8,TIM_CHANNEL_ALL);

}



//统计马达的角度和速度

void Motor_Cal(N20 *motor)
{
	int32_t dAngle=0;
	if(motor->angle-motor->lastAngle<-30000)
		dAngle=motor->angle+(65535-motor->lastAngle);
	else if(motor->angle-motor->lastAngle>30000)
		dAngle=-motor->lastAngle-(65535-motor->angle);
	else
		dAngle=motor->angle-motor->lastAngle;
	//将角度增量加入计数器
	motor->totalAngle+=dAngle;
	//计算速度
	motor->speed = (float)dAngle/(4.0*13.0*75.0)*50.0*60.0;//rpm   *50*60与定时器频率匹配
	//记录角度
	motor->lastAngle=motor->angle;
}

void Motor_GetSpeed(N20 *motor)
{
    motor->angle= __HAL_TIM_GET_COUNTER(&htim8);
    Motor_Cal(motor);
}
void Motor_SetPWM(int32_t pwm)
{
  if(pwm<0)	
	{
		__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,-pwm);
		__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,0);
	}
	else
	{
		__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,0);
		__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,pwm);
	}
}
void USER_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if(htim == &htim5)
    {
				Motor_GetSpeed(&n20);
				PID_CascadeCalc(&n20.anglePID,n20.targetAngle,n20.totalAngle,n20.speed);
				Motor_SetPWM(n20.anglePID.output);
    }
}


